#include "motor_app.h"

#include "motor_bsp_stm32.h"
#include "cmsis_os.h"
#include "tim.h"
#include "gpio.h"
#include "math.h"
#include "can.h"
#include "leg.h"

#include "dt7.h"

void Motor_Set_OutputLimit()
{
//	/* 单CAN不滤波，不丢转数据 */
//	int high_current = 8000, middle_current = 1500, low = 0;
//	int high_spd = 1200, middle_spd = 800;
//	/* 单CAN足尖滤波，不丢转数据 */
//	int high_current = 8000, middle_current = 1500, low = 0;
//	int high_spd = 2000, middle_spd = 800;
	/* 最快测试数据 */
	int high_current = 2600, middle_current = 1500, low = 0;
	int high_spd = 800, middle_spd = 600;
	switch(RC_CtrlData.rc.LeftSwitch)
	{
		case RC_SWITCH_TOP:
		{
			Leg_SetOutputMax(&LF, high_current, high_spd);
			Leg_SetOutputMax(&RF, high_current, high_spd);
			Leg_SetOutputMax(&LB, high_current, high_spd);
			Leg_SetOutputMax(&RB, high_current, high_spd);
			break;
		}
		case RC_SWITCH_MIDDLE:
		{
			
			Leg_SetOutputMax(&LF, middle_current, middle_spd);
			Leg_SetOutputMax(&RF, middle_current, middle_spd);
			Leg_SetOutputMax(&LB, middle_current, middle_spd);
			Leg_SetOutputMax(&RB, middle_current, middle_spd);
			break;
		}
		case RC_SWITCH_BOTTOM:
		{
			Leg_SetOutputMax(&LF, low, low);
			Leg_SetOutputMax(&RF, low, low);
			Leg_SetOutputMax(&LB, low, low);
			Leg_SetOutputMax(&RB, low, low);
			break;
		}
	}
}

/**
  * @brief  Limits motor output, runns PID algorithm,
  * 		and sends CAN frames to control the motor.
  * @author YanShuang
  * @retval None
  */
void Motor_Control_Task(void const * argument)
{
	/* Used in osDelayUntil */
	uint32_t PreviousWakeTime; 
	
	for(;;)
	{
		/* save wakeup time */
		PreviousWakeTime = osKernelSysTick();

		/* Limit Motor Output */
		Motor_Set_OutputLimit();

		/* Calculation task */
		for (int leg_index = 0; leg_index < 4; leg_index++)
		{
			Motor_Ctrl_Calc(&leg[leg_index].m1);
			Motor_Ctrl_Calc(&leg[leg_index].m2);
		}

		/* Send CAN command to motors */
		Motor_Cmd_Tx();
		
		osDelayUntil(&PreviousWakeTime, 2);// run every 2ms
	}
}
